#include "basic_lifecycle.h"

namespace test_utils
{
    BasicLifecycleNode::BasicLifecycleNode(const std::string &node_name, const rclcpp::NodeOptions &options)
        : rclcpp_lifecycle::LifecycleNode(node_name, options)
    {
        // Constructor implementation
    }

    bool BasicLifecycleNode::auto_check(CheckType check_type)
    {

        RCLCPP_INFO(get_logger(), "auto checking %s", get_name());
        bool result = false;

        // 1 获取当前节点状态
        uint8_t current_state;

        if (check_type == CheckType::CHECK_TO_START)
        {
            RCLCPP_INFO(get_logger(), "auto start ... ");
            current_state = get_current_state().id();
            RCLCPP_INFO(get_logger(), "current_state is %d", current_state);
            switch (current_state)
            {
            case lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN:
            case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE).id())
                {
                    check_type = CheckType::CHECK_TO_CLEANUP;
                    break;
                }
            case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE).id())
                {
                    check_type = CheckType::CHECK_TO_CLEANUP;
                    break;
                }
            case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE:
                RCLCPP_INFO(get_logger(), "node bringup succeed ");
                result = true;
                break;
            default:
                break;
            }
        }

        if (check_type == CheckType::CHECK_TO_PAUSE)
        {
            RCLCPP_INFO(get_logger(), "auto pause ... ");
            current_state = get_current_state().id();
            RCLCPP_INFO(get_logger(), "current_state is %d", current_state);
            switch (current_state)
            {
            case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE:
                result = true;
                break;
            case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE).id())
                {
                    check_type = CheckType::CHECK_TO_CLEANUP;
                }
                else
                {
                    result = true;
                    RCLCPP_INFO(get_logger(), "node pause succeed ");
                }
                break;
            case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE).id())
                {
                    check_type = CheckType::CHECK_TO_CLEANUP;
                }
                else
                {
                    result = true;
                    RCLCPP_INFO(get_logger(), "node pause succeed ");
                }
                break;
            default:
                break;
            }
        }

        if (check_type == CheckType::CHECK_TO_CLEANUP)
        {
            RCLCPP_INFO(get_logger(), "auto cleanup ... ");
            current_state = get_current_state().id();
            RCLCPP_INFO(get_logger(), "current_state is %d", current_state);
            switch (current_state)
            {
            case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE).id())
                {
                    break;
                }
            case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE:
                if (transition_state_map_[lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP] !=
                    trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP).id())
                {
                    break;
                }
            case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED:
                RCLCPP_INFO(get_logger(), "node cleanup succeed ");
                result = true;
                break;
            default:
                break;
            }
        }

        return result;
    }
}
